#include "includes.h"
#include "drivers.h"
#include "servo.h"
#include "motor.h"

extern double servo_duty;
extern double motor_duty[2];

void implementation(void){
    double tmp;

    tmp=servo_duty+SERVO_DUTY_MIDDLE;
    tmp=MIN(tmp, SERVO_DUTY_MAX);
    tmp=MAX(tmp, SERVO_DUTY_MIN);
    tpm_pwm_duty(TPM1, TPM_CH0, (int)servo_duty);

    tmp=motor_duty[0];
    if(tmp<0){
        tpm_pwm_duty(TPM0, TPM_CH0, 0);
        tpm_pwm_duty(TPM0, TPM_CH3, (int)-tmp);
    }else{
        tmp=MIN(tmp, MOTOR_DUTY_MAX);
        tpm_pwm_duty(TPM0, TPM_CH0, (int)tmp);
        tpm_pwm_duty(TPM0, TPM_CH3, 0);
    }

    tmp=motor_duty[1];
    if(tmp<0){
        tpm_pwm_duty(TPM0, TPM_CH1, 0);
        tpm_pwm_duty(TPM0, TPM_CH2, (int)-tmp);
    }else{
        tmp=MIN(tmp, MOTOR_DUTY_MAX);
        tpm_pwm_duty(TPM0, TPM_CH1, (int)tmp);
        tpm_pwm_duty(TPM0, TPM_CH2, 0);
    }
}
